#include "Arduino.h"
#include <Quaternion.h>
#include <PIcontrol.h>
#include <FP_math.h>
#include "INU.h"

INU::INU(void) {
  int zero = 0;
  gravity = 1024;
  heading = zero;
  attitude = IMU();
  gyroOffset.s.x = zero;
  gyroOffset.s.y = zero;
  gyroOffset.s.z = zero;
  accelOffset.s.x = zero;
  accelOffset.s.y = zero;
  accelOffset.s.z = zero;
  velocity.s.x = zero;
  velocity.s.y = zero;
  velocity.s.z = zero;
  bodyVelocity.s.x = zero;
  bodyVelocity.s.y = zero;
  bodyVelocity.s.z = zero;
  vIntegrator = PIVinteg();
  accelOffsetPI = PIVcontrol();
  gyroOffsetPI = PIVlowGain();
  headingPI = PIcontrol();
}



void INU::update(int accel[], int gyro[], int v_in[], int compass, int dt) {
  
  int x[3];

  // Add the gyro offset to the gyro data
  FP_math::setV(x, gyro);           // x = gyro;
  FP_math::addV(x, gyroOffset.r);   // x += gyroOffset

  // Update the attitude propagator with the gyro rates
  attitude.propagate(x, dt);        // q = q * x;

  // Update the accelerometer inputs with the accel offsets
  FP_math::setV(x, accel);
  FP_math::addV(x, accelOffset.r);

  // Convert Acceleration to NED
  attitude.body2ned(x);

  // Add acceleration due to gravity
  x[2] += gravity;

  // Integrate acceleration to velocity
  vIntegrator(x, velocity.r, dt);

  // Calculate body velocity
  FP_math::setV(bodyVelocity.r, velocity.r);
  attitude.ned2body(bodyVelocity.r);

  // Calculate velocity errors in body coordinates
  FP_math::setV(x, v_in);
  FP_math::subV(x, velocity.r);
  attitude.ned2body(x);

  // Update Accelerometer loop PI Control
  accelOffsetPI.update(x, accelOffset.r, dt);
  
  // The SVD for x,y acceleration to x,y,z gyro rates
  int qw = attitude.getW();
  int qx = attitude.getX();
  int qy = attitude.getY();
  int qz = attitude.getZ();

  int theta_x = FP_math::mult16_14(qx, qz) - FP_math::mult16_14(qw, qy);
  int theta_y = FP_math::mult16_14(qy, qz) - FP_math::mult16_14(qw, qx);

  int a_yTheta_x = FP_math::mult16_14(theta_x, accelOffset.s.y);
  int a_xTheta_y = FP_math::mult16_14(theta_y, accelOffset.s.x);

  int a_t = a_yTheta_x - a_xTheta_y;

  int a_tTheta_x = FP_math::mult16_14(a_t, theta_x);
  int a_tTheta_y = FP_math::mult16_14(a_t, theta_y);

  // The gyro error vector
  x[0] = accelOffset.s.y - a_tTheta_x;
  x[1] = a_tTheta_y - accelOffset.s.x;;
  x[3] = a_t << 1;

  // Process gyro error through gyro PI Control
  gyroOffsetPI.update(x, gyroOffset.r, dt);

  // Compute heading error
  int headErr = int((long(gyroOffset.s.z) * headGain) >> headShift);
  headErr += compass - attitude.heading();

  // Process heading error through heading PI control
  gyroOffset.s.z = headingPI.update(headErr, dt);
}
  


  
  
  
  
  